/*******************************************************************************
 * Copyright (C) 2013 Jeremy Brodt and Kevin Gillespie
 * Description: MAXQ610 Autonomous Car
 *         $Id: main.c 27 2014-04-13 22:57:20Z brodtj1212@gmail.com $
 *******************************************************************************
 */

#include <stdio.h>
#include <string.h>
#include "maxq_config.h"
#include "comm/maxq_comm.h"
#include "car.h"
#include "tick.h"
#include "motor.h"
#include "encoder.h"
#include "speed.h"
#include "dist.h"
#include "tripq.h"
#include "pos.h"
#include "wall.h"
#include "compass.h"
#include "imu.h"

/***** Definitions *****/
#define HOST_TIMEOUT  0x3FFFF

#define MAX_OBJ_CNT   2

#define checkchar()   (SCON0 & 1)

// Three rotations
#define VALUE_COMPASS_CAL_LEFT    0x1A
#define VALUE_COMPASS_CAL_RIGHT   0x17
#define DIST_COMPASS_CAL    773   // d = 82mm, 3 * pi * d = 772.8 mm

#define PB_PIN  0x04

//#define MPU_ENABLE 1
#define GZ_THRESH 800
#define IMU_BUMP 20

/***** Global Data *****/
volatile unsigned int msg_cnt;
op_mode_t motor_mode;
int speed_setpoint[NUM_WHEELS];
uint8_t motor_value[NUM_WHEELS];
unsigned int loop_overflow_cnt;
unsigned int object_detected[NUM_OBJ_SENSORS];
unsigned int object_cntr[NUM_OBJ_SENSORS];
int32_t tripstart[NUM_WHEELS];
int speed[NUM_WHEELS];
int alarm_set[NUM_WHEELS];
int tripq_running;
int tripq_complete;
int tripq_toggle;
int compass_exists;
int desired_heading;
int desired_wall_left;
int desired_wall_right;
uint8_t imu_flag;
int next_tripq_adder;

/***** Function Prototypes *****/
void display_menu(void);
void user_interface(void);
int getchar_motor_keep_alive(void);
int gethexdata(unsigned int * value);
int getdecdata(unsigned int * value);
void tripq_callback(void);
void set_tripdest(int wheel, int distance, void (*callback)(void));
void pos_log_display(void);

/****************************************************************************/
#define get_odometer_value(wheel) ((odometer[wheel] * hundred_mm_per_cnt[wheel]) / 100)
#define get_tripmeter_value(wheel) (((odometer[wheel] - tripstart[wheel]) * hundred_mm_per_cnt[wheel]) / 100)
#define get_tripremaining_value(wheel) ((enc_alarm_remaining(wheel) * hundred_mm_per_cnt[wheel]) / 100)

/****************************************************************************/
void print_i32(int32_t value)
{
  int16_t temp16a, temp16b;
  int negative;

  if (value < 0) {
    temp16b = -value % 1000;
    negative = 1;
  } else {
    temp16b = value % 1000;
    negative = 0;
  }

  if ( (value > 1000) || (value < -1000) ) {
    temp16a = value / 1000;
    maxq_printf(C" %3d", temp16a);
    maxq_printf(C"%03u", temp16b);
  }
  else {
    if (negative)
      temp16b *= -1;
    maxq_printf(C"%7d", temp16b);
  }
}

/****************************************************************************/
int main( void )
{
  uint8_t temp8, new_left, new_right, pi, new_val[NUM_WHEELS];
  volatile uint32_t startup_delay;
  int wheel, i;
  unsigned int pb_cnt;
  int setpoint_offset;

  for(startup_delay = 0; startup_delay < (uint32_t)0x3FFFF; startup_delay++);

  comm_init();
  tick_init();

  // Enable interrupts globally  
  __enable_interrupt();

  maxq_printf(C"\r\n\r\n***** Autonomous Vehicle *****\r\n");

  // Configure port for LEDs
  PD3 |= 0x03;

  // Configure port for pushbutton
  PO3 |= PB_PIN;
  PD3 &= ~PB_PIN;

  // Configure ports for object detection
  PD1 &= ~0x03;   // set ports as inputs
  PO1 |= 0x03;    // enable pull-ups

  if (!enc_init()) {
    while(1);
  }

  if (!motor_init()) {
    while(1);
  }

  if (!dist_init()) {
    maxq_printf("Failed to initialize distance sensor(s)\r\n");
  }

  if (!compass_init())
    maxq_printf("Failed to initialize compass\r\n");
  else
    compass_exists = 1;

  if (!imu_init()) {
    maxq_printf("Failed to initialize MPU\r\n");
  }

  speed_ctrl_init();

  pos_init();
  wall_init();

  // Initialize variables
  loop_overflow_cnt = 0;
  motor_mode = FIXED_VALUE;
  for (wheel = 0; wheel < NUM_WHEELS; wheel++) {
    motor_value[wheel] = 0;
    object_cntr[wheel] = 0;
    speed_setpoint[wheel] = 0;
    tripstart[wheel] = 0;
    speed[wheel] = 0;
    alarm_set[wheel] = 0;
  }
  tq_index = -1;
  object_detected[LEFT] = 0;
  object_detected[RIGHT] = 0;
  object_detected[CENTER] = 0;
  tripq_running = 0;
  tripq_complete = 0;
  desired_heading = 0;
  desired_wall_left = -1;
  desired_wall_right = -1;
  next_tripq_adder = 0;

  pb_cnt = 0;

  // Check for errors and reset timeout
  motor_error();

  while(1) {
    // 25 ms is the fastest task. Block here until its ready to run again.
    while(!flag_25ms);
    flag_25ms = 0;

    // Check if the tick alarm has expired. This is done here so it is not in the interrupt context.
    if (flag_alarm) {
      flag_alarm = 0;
      if (tick_callback != NULL) {
        tick_callback();
      }
    }

//    imu_task();
    
    // Check for all sensors for imminent collisions
    for (i = 0; i < NUM_OBJ_SENSORS; i++) {

      if (i == LEFT)
        pi = PI1 & 0x01;
      else if (i == RIGHT)
        pi = PI1 & 0x02;
#if 0
      else
        pi = (distance[CENTER] > MIN_DISTANCE);
#endif

      if ( (object_cntr[i] < MAX_OBJ_CNT) && !pi )
        object_cntr[i]++;
  
      if ( (object_cntr[i] > 0) && pi )
        object_cntr[i]--;

      // Check for obstacles and stop motors if necessary
      if ( !object_detected[i] && (object_cntr[i] == MAX_OBJ_CNT) ) {

        motor_coast(LEFT);
        motor_coast(RIGHT);

        if (i == LEFT)
          maxq_printf(C"Object LEFT!\r\n");
        else if (i == RIGHT)
          maxq_printf(C"Object RIGHT!\r\n");
        else
          maxq_printf(C"Object CENTER!\r\n");

        object_detected[i] = 1;
      }

      if ( object_detected[i] && (object_cntr[i] == 0) )
        object_detected[i] = 0;
    } // end 25 ms tasks

    if (flag_200ms) {
      flag_200ms = 0;
      PO3 ^= 1;

      // Get our current heading and position
      compass_task();
      pos_task();
    } // end 200 ms tasks

    if (flag_100ms) {
      flag_100ms = 0;

      if ( (speed[LEFT] > 300) && (motor_mode != TURNING) ) {
        setpoint_offset = pos_ctrl(desired_heading, desired_wall_left, desired_wall_right);
      } else {
        pos_ctrl_clear();
        setpoint_offset = 0;
      }
      
      // Get new motor control values
      for (wheel = 0; wheel < NUM_WHEELS; wheel++) {
        // Get current speed
        speed[wheel] = enc_get_speed(wheel);
  
        // Get new motor settings
        if (motor_mode == FIXED_VALUE) {
          new_val[wheel] = motor_value[wheel];
        } else if ( (motor_mode == FIXED_SPEED) || (motor_mode == TURNING) ) {
          if (wheel == RIGHT) {
            new_val[wheel] = speed_ctrl(wheel, speed[wheel], speed_setpoint[wheel] + setpoint_offset);
          } else {
            new_val[wheel] = speed_ctrl(wheel, speed[wheel], speed_setpoint[wheel]);
          }
        } else {
          new_val[wheel] = 0;
          motor_value[wheel] = 0;
        }
      }
      
#ifdef MPU_ENABLE
      // Handle IMU readings
      if(motor_mode == FIXED_SPEED && imu_flag){
        avg_gz = avg_gz/16;
        if (avg_gz > GZ_THRESH) {
          new_val[RIGHT]-= IMU_BUMP;            //Decrease right wheel speed
          maxq_printf("MPU Right, %d\r\n",avg_gz);
        } else if (avg_gz < (-1 * GZ_THRESH)) {
          new_val[RIGHT] += IMU_BUMP;            //Increase right wheel speed, avg_gz is negativ
          maxq_printf("MPU Left, %d\r\n",avg_gz);
        }
        
        imu_flag = 0 ;   //Use this flag to prevent over correction
      } else {
        imu_flag = 1;
      }
      
      avg_gz = 0;
#endif /* MPU_ENABLE */
      
      // Set motor control values
      if (motor_mode >= COASTING) { 
        motor_coast(LEFT);
        motor_coast(RIGHT);
        speed_ctrl_clear(LEFT, 0);
        speed_ctrl_clear(RIGHT, 0);
      }
      else if (object_detected[LEFT] || object_detected[RIGHT] || object_detected[CENTER]) { 
        motor_coast(LEFT);
        motor_coast(RIGHT);
        speed_ctrl_clear(LEFT, 0);
        speed_ctrl_clear(RIGHT, 0);
      }
      else {
        new_left = new_val[LEFT];
        new_right = new_val[RIGHT];
        __disable_interrupt();
        motor_set(LEFT, direction[LEFT], new_left);
        motor_set(RIGHT, direction[RIGHT], new_right);
        __enable_interrupt();
      }
  
      // Check for errors and reset timeout
      if ((temp8 = motor_error()) != 0) {
        maxq_printf(C"Error 0x%02x\r\n", temp8);
      }

    } // end 100 ms tasks

    if (flag_75ms) {
      flag_75ms = 0;
      dist_task();
      wall_task(desired_wall_left, desired_wall_right);
    } // end 75 ms tasks

    if (flag_400ms) {
      flag_400ms = 0;
      maxq_printf(C"md %u", motor_mode);
      maxq_printf(C" : stp %3d", tq_index);
      maxq_printf(C" : spd %4u %4u", speed[LEFT], speed[RIGHT]);
      maxq_printf(C" : set %4u %4u", speed_setpoint[LEFT], speed_setpoint[RIGHT] + setpoint_offset);

      if (motor_mode != COASTING) {
        maxq_printf(C" : trp");
        print_i32(get_tripmeter_value(LEFT));
        print_i32(get_tripmeter_value(RIGHT));
        maxq_printf(C" : dst %4d %4d %4d", distance[CENTER], distance[LEFT], distance[RIGHT]);

        if (wall_dist[LEFT] == WALL_INVALID)
          maxq_printf(C" : inv %d", wall_lock[LEFT]);
        else if (wall_dist[LEFT] == WALL_OPENING)
          maxq_printf(C" : opn %d", wall_lock[LEFT]);
        else
          maxq_printf(C" : %3d %d", wall_dist[LEFT], wall_lock[LEFT]);

        if (wall_dist[RIGHT] == WALL_INVALID)
          maxq_printf(C" inv %d", wall_lock[RIGHT]);
        else if (wall_dist[RIGHT] == WALL_OPENING)
          maxq_printf(C" opn %d", wall_lock[RIGHT]);
        else
          maxq_printf(C" %3d %d", wall_dist[RIGHT], wall_lock[RIGHT]);
      }

        maxq_printf(C" : %3d %5d", lateral_error, setpoint_offset);
//        maxq_printf(C" : ovr %2u %2u", loop_overflow_cnt, isr_overflow);
        maxq_printf(C" : %3d %3d", compass_heading / 10, rads8_to_deg(Heading));

#if 0
        maxq_printf(C" : %6d %6d", ax, ay);
        maxq_printf(C" : %6d %6d %6d", gx, gy, gz);
        maxq_printf(C" : %6d %6d", axi, ayi);
        maxq_printf(C" : %6d %6d %6d", gxi, gyi, gzi);
//        maxq_printf(C" : %4d %5d", rads8_to_deg(heading_error), setpoint_offset);
//        print_i32(X);
//        print_i32(Y);
#endif

      maxq_printf(C"\r\n");
    } // end 2nd 400 ms tasks

    // Background Tasks
    if (checkchar() || tripq_complete) {
      motor_set(LEFT, FORWARD, 0);
      motor_set(RIGHT, FORWARD, 0);

      if (tripq_complete) {
        tripq_complete = 0;
        display_menu();
      }

      user_interface();

      // Reinitialize
      speed_ctrl_clear(LEFT, 0);
      speed_ctrl_clear(RIGHT, 0);
      tq_index = tq_size - 1;

      // Clear errors
      motor_error();

      // Clear flag
      flag_100ms = 0;
    }

    // Check pushbutton input
    if (!(PI3 & PB_PIN)) {
      if (pb_cnt < 2)
        pb_cnt++;
    } else if (pb_cnt >= 2) {
      pb_cnt = 0;
      tripq_toggle = 1;
    }

    // Run/stop the trip queue if requested
    if (tripq_toggle) {
      tripq_toggle = 0;
      if (tripq_running) {
        maxq_printf(C"Stoping Trip Queue\r\n");
        for (wheel = 0; wheel < NUM_WHEELS; wheel++) {
          speed_setpoint[wheel] = 0;
          motor_coast(wheel);
          alarm_set[wheel] = 0;
        }
        tq_index = -1;
        tripq_running = 0;
        tripq_complete = 1;
      }
      else {
        maxq_printf(C"Starting Trip Queue\r\n");
        tq_index = -1;
        tripq_running = 1;
        tripq_complete = 0;
        tripq_callback();
        speed_ctrl_clear(LEFT, 0);
        speed_ctrl_clear(RIGHT, 0);
      }
    }

    if (flag_25ms) {
      loop_overflow_cnt++;
    }
  }
}

/****************************************************************************/
void tripq_callback(void)
{
  int wheel;


#if 0
  static int extended = 0;

  // Check if the next step is a turn
  if ( !extended && ((tq_index + 1) < tq_size) ) {
    if (tripq[tq_index+1].speed[LEFT] > tripq[tq_index+1].speed[RIGHT]) {
      // Turning RIGHT
      if (wall_dist[RIGHT] > 0) {
        // There is a wall, don't turn yet
        wall_alarm_set(WALL_OPEN_RIGHT, tripq_callback);
        extended = 1;
        return;
      }
    } else if (tripq[tq_index+1].speed[LEFT] < tripq[tq_index+1].speed[RIGHT]) {
      // Turning LEFT
      if (wall_dist[LEFT] > 0) {
        // There is a wall, don't turn yet
        wall_alarm_set(WALL_OPEN_LEFT, tripq_callback);
        extended = 1;
        return;
      }
    } else {
      // Going straight. Do nothing
    }
  }

  wall_alarm_clr();
#endif


  // Record lateral error from straightaway to add to next straight-away distance after turning
  if (tripq[tq_index].mode == FIXED_SPEED)  {
    next_tripq_adder = lateral_error;
  } else if (tripq[tq_index].mode == TURNING)  {
    if (tripq[tq_index].speed[LEFT] > tripq[tq_index].speed[RIGHT])
      next_tripq_adder = -next_tripq_adder;
  }

  // Increment the index
  tq_index++;
  enc_alarm_clr(LEFT);
  enc_alarm_clr(RIGHT);
  wall_alarm_clr();

  // Check if we have reached the end of the queue
  if (tq_index >= tq_size) {
    for (wheel = 0; wheel < NUM_WHEELS; wheel++) {
      speed_setpoint[wheel] = 0;
      motor_value[wheel] = 0;
      motor_coast(wheel);
      enc_alarm_clr(wheel);
      alarm_set[wheel] = 0;
    }
    desired_heading = 0;
    desired_wall_left = -1;
    desired_wall_right = -1;
    tripq_running = 0;
    tripq_complete = 1;
    return;
  }

  // Load next trip information  
  motor_mode = tripq[tq_index].mode;
  desired_heading = tripq[tq_index].heading;
  desired_wall_left = tripq[tq_index].wall[LEFT];
  desired_wall_right = tripq[tq_index].wall[RIGHT];

  if (tripq[tq_index].mode == PAUSE) {
      motor_coast(LEFT);
      motor_coast(RIGHT);
      tick_alarm_set(500, tripq_callback);
  } else {
    for (wheel = 0; wheel < NUM_WHEELS; wheel++) {
      enc_direction_change(wheel, tripq[tq_index].direction[wheel]);
  
      if (tripq[tq_index].mode == FIXED_VALUE) {
        motor_value[wheel] = tripq[tq_index].speed[wheel];
        set_tripdest(wheel, tripq[tq_index].distance[wheel], tripq_callback);
      } else if (tripq[tq_index].mode == FIXED_SPEED) {
        speed_setpoint[wheel] = tripq[tq_index].speed[wheel];
        set_tripdest(wheel, tripq[tq_index].distance[wheel] + next_tripq_adder, tripq_callback);
      } else {
        speed_setpoint[wheel] = tripq[tq_index].speed[wheel];
        set_tripdest(wheel, tripq[tq_index].distance[wheel], tripq_callback);
      }
    }
  }
}

/****************************************************************************/
void end_callback(void)
{
  int wheel;
  for (wheel = 0; wheel < NUM_WHEELS; wheel++) {
    speed_setpoint[wheel] = 0;
    motor_value[wheel] = 0;
    motor_coast(wheel);
    enc_alarm_clr(wheel);
    alarm_set[wheel] = 0;
  }
}

/****************************************************************************/
void compass_cal_callback(void)
{
  int wheel;
  for (wheel = 0; wheel < NUM_WHEELS; wheel++) {
    speed_setpoint[wheel] = 0;
    motor_value[wheel] = 0;
    motor_coast(wheel);
    enc_alarm_clr(wheel);
    alarm_set[wheel] = 0;
  }

  if (compass_cal_end())
    maxq_printf(C"Compass calibration compete\r\n");
}

/****************************************************************************/
void set_tripdest(int wheel, int distance, void (*callback)(void))
{
  int32_t count;
  count = (int32_t)distance * 100;
  count += (hundred_mm_per_cnt[wheel] >> 1);
  count /= hundred_mm_per_cnt[wheel];
  if (callback != NULL)
    enc_alarm_set(wheel, count, callback);
  else
    enc_alarm_set(wheel, count, end_callback);
  tripstart[wheel] = odometer[wheel];
  alarm_set[wheel] = 1;
}

/****************************************************************************/
int check_alarms_set(void)
{
  if ( (motor_mode == FIXED_VALUE) && ((motor_value[LEFT] > 0) || (motor_value[RIGHT] > 0)) && (!alarm_set[LEFT] || !alarm_set[RIGHT]) ) {
    maxq_printf(C"No alarm set\r\n");
    return 0;
  }

  if ( (motor_mode == FIXED_SPEED) && (((speed_setpoint[LEFT] > 0) && !alarm_set[LEFT]) || ((speed_setpoint[RIGHT] > 0) && !alarm_set[RIGHT])) ) {
    maxq_printf(C"No alarm set\r\n");
    return 0;
  }

  return 1;
}

/****************************************************************************/
void display_menu(void)
{
  maxq_printf(C"Overflows %u\r\n", loop_overflow_cnt);
  maxq_printf(C"\r\n### MENU ###\r\n");
  if (motor_mode == FIXED_VALUE) {
    maxq_printf(C"  m - Set motor value (0x%02x 0x%02x)\r\n", motor_value[LEFT], motor_value[RIGHT]);
    maxq_printf(C"  s - Set speed\r\n");
  } else {
    maxq_printf(C"  m - Set motor value\r\n");
    maxq_printf(C"  s - Set speed (%u %u)\r\n", speed_setpoint[LEFT], speed_setpoint[RIGHT]);
  }
  maxq_printf(C"  f - Go forward (");
  print_i32(get_tripremaining_value(LEFT));
  print_i32(get_tripremaining_value(RIGHT));
  maxq_printf(C")\r\n");
  maxq_printf(C"  t - Turn\r\n");
  maxq_printf(C"  r - Rotate\r\n");
  maxq_printf(C"  o - Coast\r\n");
  maxq_printf(C"  l - Display speed Log\r\n");
  maxq_printf(C"  p - Display Position log\r\n");
  maxq_printf(C"  y - Display encoder log\r\n");
  maxq_printf(C"  z - Display first encoder log\r\n");
  maxq_printf(C"  e - Erase logs\r\n");
  maxq_printf(C"  a - Calibrate compass\r\n");
  maxq_printf(C"  d - Distance log\r\n");
  maxq_printf(C"  q - Run TRIPQ\r\n");
  maxq_printf(C"  c - Continue\r\n");
}

/****************************************************************************/
int getchar_motor_keep_alive()
{
  while(!checkchar()) {

    if (flag_100ms) {
      flag_100ms = 0;
      // Check for errors and reset timeout
      motor_error();
    }

    if (flag_200ms) {
      flag_200ms = 0;
      PO3 ^= 1;
    }
  }

  return getchar();
}

/****************************************************************************/
void user_interface()
{
  char cmd, tempch;
  unsigned int value;
  int firstpass = 1;

  do {
    cmd = getchar_motor_keep_alive();
    maxq_printf(C"\r\n");

    switch (cmd) {
      case 'm':
        if (gethexdata(&value)) {
          motor_mode = FIXED_VALUE;
          motor_value[LEFT] = value;
          motor_value[RIGHT] = value;
        }
        break;
      case 's':
        if (getdecdata(&value)) {
          motor_mode = FIXED_SPEED;
          speed_setpoint[LEFT] = value;
          speed_setpoint[RIGHT] = value;
        }
        break;
      case 'f':
        maxq_printf(C"How far in mm?\r\n");
        if (getdecdata(&value)) {
          enc_direction_change(LEFT, FORWARD);
          enc_direction_change(RIGHT, FORWARD);
          set_tripdest(LEFT, value, end_callback);
          set_tripdest(RIGHT, value, end_callback);
        }
        break;
      case 'r':
        maxq_printf(C"Which direction? (r/l): ");
        tempch = getchar_motor_keep_alive();
        maxq_printf(C"\r\n");
        if ((tempch == 'r') || (tempch == 'R')) {
          enc_direction_change(LEFT, FORWARD);
          enc_direction_change(RIGHT, REVERSE);
          set_tripdest(LEFT, DIST_ROTATE_90_DEG, end_callback);
          set_tripdest(RIGHT, -DIST_ROTATE_90_DEG, end_callback);
          motor_mode = FIXED_VALUE;
          motor_value[LEFT] = VALUE_ROTATE;
          motor_value[RIGHT] = VALUE_ROTATE;
        } else if ((tempch == 'l') || (tempch == 'L')) {
          enc_direction_change(LEFT, REVERSE);
          enc_direction_change(RIGHT, FORWARD);
          set_tripdest(LEFT, -DIST_ROTATE_90_DEG, end_callback);
          set_tripdest(RIGHT, DIST_ROTATE_90_DEG, end_callback);
          motor_mode = FIXED_VALUE;
          motor_value[LEFT] = VALUE_ROTATE;
          motor_value[RIGHT] = VALUE_ROTATE;
        } else {
          maxq_printf(C"Invalid character\r\n");
        }
        break;
      case 't':
        maxq_printf(C"Which direction? (r/l): ");
        tempch = getchar_motor_keep_alive();
        maxq_printf(C"\r\n");
        if ((tempch == 'r') || (tempch == 'R')) {
          enc_direction_change(LEFT, FORWARD);
          enc_direction_change(RIGHT, FORWARD);
          set_tripdest(LEFT, ARC_OUTSIDE, end_callback);
          set_tripdest(RIGHT, ARC_INSIDE, end_callback);
          motor_mode = TURNING;
          speed_setpoint[RIGHT] = in_whl_spd(speed_setpoint[LEFT]);
        } else if ((tempch == 'l') || (tempch == 'L')) {
          enc_direction_change(LEFT, FORWARD);
          enc_direction_change(RIGHT, FORWARD);
          set_tripdest(LEFT, ARC_INSIDE, end_callback);
          set_tripdest(RIGHT, ARC_OUTSIDE, end_callback);
          motor_mode = TURNING;
          speed_setpoint[LEFT] = in_whl_spd(speed_setpoint[RIGHT]);
        } else {
          maxq_printf(C"Invalid character\r\n");
        }
        break;
      case 'o':
        motor_mode = COASTING;
        motor_value[LEFT] = 0;
        motor_value[RIGHT] = 0;
        motor_coast(LEFT);
        motor_coast(RIGHT);
        maxq_printf(C"Continuing...\r\n");
        return;
      case 'l':
        speed_log_display();
        break;
      case 'p':
        pos_log_display();
        break;
      case 'y':
        enc_log_display();
        break;
      case 'z':
        enc_log_display_first();
        break;
      case 'a':
        if (!compass_cal_start()) {
          maxq_printf(C"Failed to start compass calibration\r\n");
        } else {
          maxq_printf(C"Calibrating compass...\r\n");
          enc_direction_change(LEFT, FORWARD);
          enc_direction_change(RIGHT, REVERSE);
          set_tripdest(LEFT, DIST_COMPASS_CAL, compass_cal_callback);
          set_tripdest(RIGHT, -DIST_COMPASS_CAL, compass_cal_callback);
#if 1
          motor_mode = FIXED_VALUE;
          motor_value[LEFT] = VALUE_COMPASS_CAL_LEFT;
          motor_value[RIGHT] = VALUE_COMPASS_CAL_RIGHT;
#else
          motor_mode = FIXED_SPEED;
          speed_setpoint[LEFT] = 100;
          speed_setpoint[RIGHT] = 100;
#endif
          return;
        }
        break;
      case '0':
      case '1':
      case '2':
      case '3':
        dist_mode = cmd - 0x30;
        maxq_printf(C"Distance mode set to %d\r\n", cmd - 0x30);
        break;
      case 'e':
        speed_log_clear();
        break;
      case 'd':
        dist_log_display();
        break;
      case 'q':
        tripq_toggle = 1;
        return;
      case 'c':
        if (check_alarms_set()) {
          if (!firstpass)
            maxq_printf(C"Continuing...\r\n");
          return;
        }
        break;
      case 0x1B:
        if (!firstpass && check_alarms_set()) {
          maxq_printf(C"Continuing...\r\n");
          return;
        }
        break;
      default:
        display_menu();
        break;
    }

    firstpass = 0;
    maxq_printf(C"Enter command:");
  } while (1);
}

/****************************************************************************
* Gets a hexidecimal value from the user interface
****************************************************************************/
int gethexdata(unsigned int * value)
{
  int i = 0;
  int ch;

  *value = 0;

  maxq_printf(C"Enter hex value: 0x");

  while(1) {

    ch = getchar_motor_keep_alive();

    if ((ch >= '0') && (ch <= '9'))
      ch = (ch - '0');
    else if ((ch >= 'A') && (ch <= 'F'))
      ch = (ch - ('A' - 0xA));
    else if ((ch >= 'a') && (ch <= 'f'))
      ch = (ch - ('a' - 0xA));
    else if ( (ch == '\r') || (ch == '\n') )
      break;
    else if (ch == 0x1B)
      return 0;
    else {
      maxq_printf(C"\r\nInvalid charater (0x%02x)\r\n", ch);
      return 0;
    }

    if (ch >= 0) {
      *value <<= 4;
      *value |= ch;

      if (++i >= (sizeof(*value) << 1))
        break;
    }
  }

  maxq_printf(C"\r\n");

  return 1;
}

/****************************************************************************
* Gets a decimal value from the user interface
****************************************************************************/
int getdecdata(unsigned int * value)
{
  int ch;

  *value = 0;

  maxq_printf(C"Enter decimal value: ");

  while(1) {

    ch = getchar_motor_keep_alive();

    if ((ch >= '0') && (ch <= '9'))
      ch = (ch - '0');
    else if ( (ch == '\r') || (ch == '\n') )
      break;
    else if (ch == 0x1B)
      return 0;
    else {
      maxq_printf(C"\r\nInvalid charater (0x%02x)\r\n", ch);
      return 0;
    }

    if (ch >= 0) {
      *value *= 10;
      *value += ch;
    }
  }

  maxq_printf(C"\r\n");

  return 1;
}
